//
// Created by bourne on 2024/4/20.
//

// todo

#include "active_perception/graph_node.h"
#include "path_searching/astar2.h"
#include "plan_env/sdf_map.h"
#include "plan_env/raycast.h"

namespace fast_planner {
// Static data
double ViewNode::vm_;
double ViewNode::am_;
double ViewNode::yd_;
double ViewNode::ydd_;
double ViewNode::w_dir_;
shared_ptr<Astar> ViewNode::astar_;
shared_ptr<RayCaster> ViewNode::caster_;
shared_ptr<SDFMap> ViewNode::map_;

// Graph node for viewpoints planning
ViewNode::ViewNode(const Vector3d& p, const double& y) {
  pos_ = p;
  yaw_ = y;
  parent_ = nullptr;
  vel_.setZero();  // vel is zero by default, should be set explicitly
}

double ViewNode::costTo(const ViewNode::Ptr& node) {
  vector<Vector3d> path;
  double c = ViewNode::computeCost(pos_, node->pos_, yaw_, node->yaw_, vel_, yaw_dot_, path);
  // std::cout << "cost from " << id_ << " to " << node->id_ << " is: " << c << std::endl;
  return c;
}

// 这段代码是 ViewNode 类的 searchPath 静态成员函数的实现，用于在两点之间寻找路径。
double ViewNode::searchPath(const Vector3d& p1, const Vector3d& p2, vector<Vector3d>& path) {
  // Try connect two points with straight line
  // 尝试用直线连接两个点
  bool safe = true; // 假设路径是安全的
  Vector3i idx; // 用于存储射线投射器返回的索引
  caster_->input(p1, p2); // 射线投射器初始化，设置起点和终点
  while (caster_->nextId(idx)) {
    if (map_->getInflateOccupancy(idx) == 1 || !map_->isInBox(idx)) {
      // map_->getOccupancy(idx) == SDFMap::UNKNOWN
      safe = false;
      break;
    }
  }
  if (safe) {
    path = { p1, p2 };
    return (p1 - p2).norm();
  }

  // Search a path using decreasing resolution
  // 如果直线路径不安全，使用A*算法搜索路径
  vector<double> res = { 0.4 }; // 使用递减分辨率搜索路径 // 定义分辨率数组
  for (int k = 0; k < res.size(); ++k) {
    astar_->reset();
    astar_->setResolution(res[k]);
    if (astar_->search(p1, p2) == Astar::REACH_END) {
      path = astar_->getPath();
      return astar_->pathLength(path);
    }
  }
  // Use Astar early termination cost as an estimate
  // 如果A*算法未能找到路径，使用A*算法的提前终止代价作为估计
  path = { p1, p2 }; // 将两点添加到路径中作为直线路径
  return 100; // 返回一个较大的估计代价
}

// 这段代码实现了 ViewNode 类的 computeCost 成员函数，用于计算两个视点节点之间的转移代价。
double ViewNode::computeCost(const Vector3d& p1, const Vector3d& p2, const double& y1,
                             const double& y2, const Vector3d& v1, const double& yd1, vector<Vector3d>& path) {
  // Cost of position change
  // 计算位置变化的代价
  // searchPath 函数用于在 p1 和 p2 之间搜索路径，并计算路径长度
  // 将路径长度除以速度参数 vm_ 得到位置代价
  double pos_cost = ViewNode::searchPath(p1, p2, path) / vm_;

  // Consider velocity change // 考虑速度变化对代价的影响
  if (v1.norm() > 1e-3) {
    Vector3d dir = (p2 - p1).normalized();
    Vector3d vdir = v1.normalized();
    double diff = acos(vdir.dot(dir)); // 计算速度向量与方向向量之间的夹角
    pos_cost += w_dir_ * diff;
    // double vc = v1.dot(dir);
    // pos_cost += w_dir_ * pow(vm_ - fabs(vc), 2) / (2 * vm_ * am_);
    // if (vc < 0)
    //   pos_cost += w_dir_ * 2 * fabs(vc) / am_;
  }

  // Cost of yaw change // 计算偏航角变化的代价
  double diff = fabs(y2 - y1); // 两个偏航角之间的差的绝对值
  diff = min(diff, 2 * M_PI - diff); // 如果偏航角差的绝对值大于π，则使用2π减去偏航角差来计算代价
  double yaw_cost = diff / yd_; // 将偏航角差异除以偏航角速度参数 yd_ 得到偏航角代价

  // 返回总代价，这里使用 max 函数来确保至少有一种代价被计算
  // 这种计算方式倾向于选择偏航角变化较小或位置变化较小的路径
  return max(pos_cost, yaw_cost);

  // // Consider yaw rate change
  // if (fabs(yd1) > 1e-3)
  // {
  //   double diff1 = y2 - y1;
  //   while (diff1 < -M_PI)
  //     diff1 += 2 * M_PI;
  //   while (diff1 > M_PI)
  //     diff1 -= 2 * M_PI;
  //   double diff2 = diff1 > 0 ? diff1 - 2 * M_PI : 2 * M_PI + diff1;
  // }
  // else
  // {
  // }
}
}